
import sys
sys.path=sys.path + [	
	'z:/tingfan/win-ros-sdk-build/src/ros/core/roslib/src',  #roslib
	'z:/tingfan/win-ros-sdk-build/build/release/gen/py/', #stdmsgs
	'z:/tingfan/win-ros-sdk-build/src/ros_comm/clients/rospy/src/', #rospy
	'src/', #Doubles.py
	'pyNode' #JointIO.py
		]
print sys.path

import numpy
import rospy
import socket
from std_msgs.msg import String
from nidaqIO.msg import Doubles
from JointIO import JointIO

###########################################################################
def setpriority(pid=None,priority=1):
    """ Set The Priority of a Windows Process.  Priority is a value between 0-5 where
        2 is normal priority.  Default sets the priority of the current
        python process but can take any valid process ID. """
        
    import win32api,win32process,win32con
    
    priorityclasses = [win32process.IDLE_PRIORITY_CLASS,
                       win32process.BELOW_NORMAL_PRIORITY_CLASS,
                       win32process.NORMAL_PRIORITY_CLASS,
                       win32process.ABOVE_NORMAL_PRIORITY_CLASS,
                       win32process.HIGH_PRIORITY_CLASS,
                       win32process.REALTIME_PRIORITY_CLASS]
    if pid == None:
        pid = win32api.GetCurrentProcessId()
    handle = win32api.OpenProcess(win32con.PROCESS_ALL_ACCESS, True, pid)
    win32process.SetPriorityClass(handle, priorityclasses[priority])


class SubListener(rospy.SubscribeListener):
	def peer_subscribe(self, topic, topic_pub, peer_pub):
		print "New connection: set_tcp_nodelay"
		self.pub.impl.connections[-1].socket.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)

#############################################################################
setpriority(priority=4)
rospy.init_node('pyWinNidaqIO')
class JointIOStateful(JointIO):
	def __init__(self):
		super(JointIOStateful,self).__init__();
		self.writeBuffer=numpy.ones(88)*3
		self.write(self.writeBuffer);

	def write(self, data):
		idx=~numpy.isnan(data);
		self.writeBuffer[idx] = data[idx];
		super(JointIOStateful, self).write(self.writeBuffer);

J=JointIOStateful();

def cbwrite(msg):
	print "Writes:", msg.data
	J.write(numpy.array(msg.data))
sub=rospy.Subscriber('/diego/writeraw', Doubles, cbwrite, queue_size=1, tcp_nodelay=True)

#The listener force all new subscribing connections to be TCP_NODELAY
sl=SubListener()
pub=rospy.Publisher('/diego/readraw', Doubles, sl, tcp_nodelay=True)
sl.pub=pub;
count=0

msg=Doubles();
while not  rospy.is_shutdown():
	data=J.read();
	msg.header.seq=count; count+=1
	msg.header.stamp=rospy.Time.now()
	msg.data=data;
	pub.publish(msg)
	count+=1
